//#include <CMPS03.h>
#include <Movement.h> // graph อยู่บรรทัด 
#include <Graph2.h>
#include <QueueList.h>
#include <QueueList2.h> // QueueList 2 is the QueueList for Coordinate class
#include <Robot.h>
#include <Mazemove.h>
#include <Coordinate.h>
#define REF 400  //Reference for sensor(reflector reading)  if sensorRead<REF-> black
// movement condition                                       if sensorRead>REF->white
#define sen_FW (sReadL>REF&&sReadM>REF&&sReadFR>REF)    //condition for move forward
#define sen_Right (sReadL<REF&&sReadM>REF&&sReadR>REF) //right
#define sen_Left (sReadL>REF&&sReadM>REF&&sReadR<REF)  //left
#define reachWhiteNode (sReadL3>REF&&sReadM>REF&&sReadR3>REF) //condition for white node
#define reachBlackNode (sReadL3>REF&&sReadM<REF&&sReadR3>REF) //condition for black node
#define reachNode (sReadL3>REF&&sReadR3>REF) //can be black or white node 
#define leftBridge (sReadL3>REF&&sReadR3<REF) //condition for leftbridge detection
#define rightBridge (sReadL3<REF&&sReadR3>REF) //condition for rightbridge detection
//reflector sensor
#define senL A0 //left                                       
#define senM A1 //middle                               
#define senR A2 //right                            
#define senL2 A3 //far left                         
#define senR2 A4 //far right    
#define senL3 A5
#define senR3 A6
#define senFL A7
#define senFR A8
#define senB A9
#define senT A10
//led output to check sensor
#define LED_L 22 //left   //digital                
#define LED_M 23 //middle                              
#define LED_R 24 //right                            
#define LED_L2 25 //far left                         
#define LED_R2 26 //far right    
#define LED_L3 27
#define LED_R3 28
#define LED_FL 29
#define LED_FR 30
#define LED_B 31
#define LED_T 32
//motor's LED should be analog
//#define LED_MotorL 6 //PWM
//#define LED_MotorR 8
//switch
#define switch_D 40 //D4 dry run switch , it'll perform dryrun when pressed
#define switch_M 41 //maze solving switch, it'll do the maze solving when pressed
//power
#define lowPower 25
#define mediumPower 30 
#define highPower 35
// motor output          
#define motorL_A 22      // use H-bridge
#define motorL_B 23      //  move forward L1,R1=1, L2,R2=0
#define motorR_A 24      //  move backward L1,R1=0, L2,R2=1
#define motorR_B 25      //
#define motorL_P 8      //PWM
#define motorR_P 9      //PWM
//time
#define small 5
#define threshold 50
#define threshold_turn 30
//graph
#define dimx 2 //only for test naja -> in real competition, change both to 8
#define dimy 4
Graph2 graph;
//movement and robot declaration
Movement movement;
Mazemove mazemove;
Robot robot;
// sensor read
int sReadL3,sReadL2,sReadL,sReadM,sReadR,sReadR2,sReadR3,sReadFL,sReadFR,sReadB,sReadT;
//sensor reading
void readSensors(){ // prepare info for trackline
  sReadL3 = analogRead(senL3);
  if(sReadL3>REF)  
    digitalWrite(LED_L3,HIGH); //LED pin will be high when that sensor is on white line
  else
    digitalWrite(LED_L3,LOW);  
  sReadL2 = analogRead(senL2);
   if(sReadL2>REF)  
    digitalWrite(LED_L2,HIGH); //LED pin will be high when that sensor is on white line
  else
    digitalWrite(LED_L2,LOW);  
  sReadL = analogRead(senL);
   if(sReadL3>REF)  
    digitalWrite(LED_L3,HIGH); //LED pin will be high when that sensor is on white line
  else
    digitalWrite(LED_L3,LOW);  
  sReadM = analogRead(senM);
     if(sReadM>REF)  
    digitalWrite(LED_M,HIGH); //LED pin will be high when that sensor is on white line
  else
    digitalWrite(LED_M,LOW);  
  sReadR = analogRead(senR);
   if(sReadR>REF)  
    digitalWrite(LED_R,HIGH); //LED pin will be high when that sensor is on white line
  else
    digitalWrite(LED_E,LOW);  
  sReadR2 = analogRead(senR2);
   if(sReadR2>REF)  
    digitalWrite(LED_R2,HIGH); //LED pin will be high when that sensor is on white line
  else
    digitalWrite(LED_R2,LOW);  
  sReadR3 = analogRead(senR3);
   if(sReadR3>REF)  
    digitalWrite(LED_R3,HIGH); //LED pin will be high when that sensor is on white line
  else
    digitalWrite(LED_R3,LOW);  
  sReadFL = analogRead(senL);
   if(sReadFL>REF)  
    digitalWrite(LED_FL,HIGH); //LED pin will be high when that sensor is on white line
  else
    digitalWrite(LED_FL,LOW);  
  sReadFR = analogRead(senM);
   if(sReadFR>REF)  
    digitalWrite(LED_FR,HIGH); //LED pin will be high when that sensor is on white line
  else
    digitalWrite(LED_FR,LOW);  
}

int checkBox(){ //check the type of the box
  sReadB = analogRead(senB);
  sReadT = analogRead(senT);
  if(sReadB<REF&&sReadT<REF){
    return 0; //no box
  }
  if(sReadB>REF&&sReadT>REF){
    return 1; //typeA
  }
  if(sReadB<REF&&sReadT<REF){
    return 2; //typeB
  }
}

void move_fw_wo_err(int time,int pow,Movement movement){ //move forward with calibration, time should be small
  if(sen_FW){
    movement.forward(time,pow); //ตรง
  }
  else if(sen_Right){
    movement.right(time,pow); //หลุดซ้ายให้เลี้ยวขวา
  }
  else if(sen_Left){
    movement.left(time,pow); //หลุดขวาให้เลี้ยวซ้าย
  }
  else{
    movement.forward(time,pow); //อยู่บน node ให้เดินตรง
  }
}

void move_backward_wo_err(int time,int pow,Movement movement){ //move backward with calibration, time should be small
  if(sen_FW){
    movement.backward(time,pow);
  }
  else if(sen_Right){ // หลุดซ้าย
    movement.backright(time,pow); 
  }
  else if(sen_Left){ //หลุดขวา
    movement.backleft(time,pow); 
  }
  else{
    movement.backward(time,pow);
  }
}

void moveBackUntilReachNode(Movement movement){ //เอาไว้ใช้ตอนเลี้ยวเพราะตอนเลี้ยวมันจะเลี้ยวเลย node ไป
  while(!reachNode){
    move_backward_wo_err(small,mediumPower,movement);
  }
  movement.stop(small); 
}
//ตอนแรกให้วางโดยsensorแถบหน้า อยู่ตรงnode
void moveAndUpdate(int x,int y,int face_Dir,Graph2 graph,Movement movement){ //this function will update the graph for each run to the adjacent node-> used for dryRun
  int nextX = graph.nextX(x,face_Dir); 
  int nextY = graph.nextY(y,face_Dir);
  Coordinate nextNode = graph.node[nextX][nextY]; // หาว่าค่า nextNode อยู่ตรงไหน โดยดูจาก face direction หรือทิศที่หุ่นหันหน้าไป 
  int time1=0,time2=0; 
  int checktime=0;
  boolean acrossBridge2=false,acrossBridge3=false,hasLeftBridge=false,hasRightBridge=false; //across bridge หรือยัง
  readSensors();
  while(reachNode){       // ตอนที่ยังอยู่บนnodeให้เดินพ้นไปก่อน
    move_fw_wo_err(small,mediumPower,movement);
    readSensors();
  }
  while(sReadL3>REF||sReadR3>REF){ //ถ้า sensor ทั้งคู่ยังไม่พ้น node เริ่มต้นให้เดินให้พ้นไปก่อน
   move_fw_wo_err(small,mediumPower,movement);
    readSensors();
  }  
 
  while(!reachNode){ //while the nextnode is not reached, we check whether it has bridge or not along the way
    move_fw_wo_err(small,mediumPower,movement);
    readSensors();
    if(/*sReadL3<REF&&sReadR3<REF&&*/!acrossBridge3){
      time1++; //time1 is the duration before L3 or R3 detect bridge    
    }  
    if(/*sReadL2<REF&&sReadR2<REF&&*/!acrossBridge2){
      time2++; //time1 is the duration before L2 or R2 detect bridge
    }
   //(sReadL3>REF&&sReadR3<REF)
    while(leftBridge){
      move_fw_wo_err(small,mediumPower,movement);
      readSensors();
      checktime++;
      if(checktime>threshold){ //ตอนที่checkได้ว่ามี bridge ซ้ายต้องเอาเวลา checktimeเข้าช่วยเพราะเราจะไม่รู้ว่ามันเป็นกรณีที่อยู่บนเส้น หรือกรณีที่กำลังจะเข้าไปใน node   
        hasLeftBridge=true;    //ถ้ากำลังจะเข้าไปใน node จะเป็นกรณีที่ L3 เข้าถึงเส้นขาวก่อน R3 แปบเดียวแล้ว loopนี้จะวนไม่กี่รอบ ในขณะที่ถ้าอยู่bridge จิง loopนี้จะวนนานหน่อย    
        acrossBridge3=true;     //จึงต้องมี parameter checktime ช่วยกะ threshold ว่าต้องวน loopนี้อย่างน้อยกี่รอบถึงจะรู้ว่าอยู่บน bridge
      } //acrossBridge3 คือ sensorที่ตำแหน่ง3 ผ่าน bridge แล้ว
    }
    //(sReadL3<REF&&sReadR3>REF)
    while(rightBridge){
      move_fw_wo_err(small,mediumPower,movement);
      readSensors();
      checktime++;
      if(checktime>threshold){       
        hasRightBridge=true;
        acrossBridge3=true;
      }
    }
    //while detect leftbridge using L2
    while(sReadL2>REF&&sReadR2<REF){
      move_fw_wo_err(small,mediumPower,movement);
      readSensors();
      checktime++;
      if(checktime>threshold){   
        acrossBridge2=true;     
      } //acrossBridge2 คือ sensorที่ตำแหน่ง3 ผ่าน bridge แล้ว
    }
    //while detect rightbridge using L2
    while(sReadL2<REF&&sReadR2>REF){
      move_fw_wo_err(small,mediumPower,movement);
      readSensors();
      checktime++;
      if(checktime>threshold){       
        acrossBridge2=true;
      }
    }
  }
  //when next node is reached
  //if(reachWhiteNode){nextNode.setEmpty(true);} -> ไม่ต้องมีเพราะมัน trueอยู่แล้ว
  if(reachBlackNode){
    graph.setBlackNode(nextX,nextY);
    Serial.println("reachBlackNode");
  }  
  //begin to adjust the graph. All we have to do is 1.blackNode? or 2.bridge
  if(face_Dir==4){   //diagonal edges will have 2 types
    if(hasLeftBridge){                                              
      if(time1>time2&&graph.node[nextX][nextY].empty=='e'){ //see description for diagonal edge type        
       graph.mapedge[nextX][nextY][1]=2;
       Serial.print("x=");
       Serial.print(nextX);
       Serial.print("  y=");
       Serial.print(nextY);
       Serial.println(" edge1=type2");
        }
      if(time1<time2&&graph.node[x][y].empty=='e'){  //3 type1
        graph.mapedge[x][y][3]=1;
       Serial.print("x=");
       Serial.print(x);
       Serial.print("  y=");
       Serial.print(y);
       Serial.println(" edge3=type1");
      }
    }
    if(hasRightBridge){
      if(time1>time2&&graph.node[nextX][nextY].empty=='e'){ //7type2
         graph.mapedge[nextX][nextY][7]=2;
       Serial.print("x=");
       Serial.print(nextX);
       Serial.print("  y=");
       Serial.print(nextY);
       Serial.println(" edge7=type2");
          }
      if(time1<time2&&graph.node[x][y].empty=='e'){ //5 type 1
        graph.mapedge[x][y][5]=1;
        }
     }  
  }

  if(face_Dir==2){
    if(hasLeftBridge){
      if(time1>time2&&graph.node[nextX][nextY].empty=='e'){ //7 type 1
       graph.mapedge[nextX][nextY][7]=1;
       }
      if(time1<time2&&graph.node[x][y].empty=='e'){//1 type 1
        graph.mapedge[x][y][1]=1;
        }
    }
    if(hasRightBridge){
      if(time1>time2&&graph.node[nextX][nextY].empty=='e'){ //5 type2
        graph.mapedge[nextX][nextY][5]=2;
       }
      if(time1<time2&&graph.node[x][y].empty=='e'){ //3 type2
        graph.mapedge[x][y][3]=2;
       }
    }  
  }
  if(face_Dir==0){
    if(hasLeftBridge){
      if(time1>time2&&graph.node[nextX][nextY].empty=='e'){ //5 type 1
        graph.mapedge[nextX][nextY][5]=1;
        }
      if(time1<time2&&graph.node[x][y].empty=='e'){ //7 type 2
        graph.mapedge[x][y][7]=2;
        }
    }
    if(hasRightBridge){
      if(time1>time2&&graph.node[nextX][nextY].empty=='e'){ //3type1
        graph.mapedge[nextX][nextY][3]=1;
        }
      if(time1<time2&&graph.node[x][y].empty=='e'){ //2type2
        graph.mapedge[x][y][1]=2;
        }
    }  
  }
}// end of move and update
//used for maze
void Turn_Dryrun(int old_Dir, int new_Dir,int pow,Movement movement){ //turn 45 or turn 90 or 180
if((old_Dir==2&&new_Dir==0)||(old_Dir==0&&new_Dir==6)||(old_Dir==6&&new_Dir==4)||(old_Dir==4&&new_Dir==2)){
  //turnleft
  int timecount=0;
  readSensors();
  while(reachNode){ //sRead_L3 && R3 = white
    movement.left(small,pow);
    readSensors();
    }
  while(sReadL>REF){
   movement.left(small,pow);
   readSensors();
  }
  while(timecount<threshold_turn){ // threshold turn คือ เวลาที่ใช้ในการเดินตรงไปข้างหน้าเพื่อปรับให้แนวหุ่นอยู่ในแนวเดียวกันกับ เส้น
   move_fw_wo_err(small,pow,movement);
   readSensors();
   timecount++;
  } 
  movement.stop(small);  
}
  //rotate right 90
  if((old_Dir==0&&new_Dir==2)||(old_Dir==6&&new_Dir==0)||(old_Dir==4&&new_Dir==6)||(old_Dir==2&&new_Dir==4)){
     int timecount=0;
  readSensors();
  while(reachNode){ //sRead_L3 && R3 = white
    movement.right(small,mediumPower);
    readSensors();
    }
  while(sReadR>REF){
   movement.right(small,mediumPower);
   readSensors();
  }
  while(timecount<threshold_turn){
   move_fw_wo_err(small,mediumPower,movement);
   readSensors();
   timecount++;
  } 
  movement.stop(small);  
    //turn right
  }

}
int  walk(QueueList<String> pathDes,Mazemove mazemove){ //return final face_direction and walk according to path description
  String final_des;                                      // see pathDescription algorithm in Graph2
  for(int i=0;i<pathDes.count()-1;i++){
   String description = pathDes.getItem(i);
  if(description=="left90")
   mazemove.goLeft90();
  else if(description=="right90")
   mazemove.goRight90();
  else if(description=="left45")
   mazemove.goLeft45(); 
  else if(description=="right45")
   mazemove.goRight45(); 
  else if(description=="uturn")
   mazemove.uTurnLeft(); 
  else if(description=="straight1")
   mazemove.forward_n(1); 
  else if(description=="straight2")
  mazemove.forward_n(2); 
  else if(description=="straight3")
   mazemove.forward_n(3);  
  else if(description=="straight4")
   mazemove.forward_n(4);
   else if(description=="straight5")
   mazemove.forward_n(5);   
 else if(description=="straight6")
   mazemove.forward_n(6);   
 else if(description=="straight7")
   mazemove.forward_n(7);
 else
  Serial.print("what the??"); 
 }
 mazemove.stop(); //stop or brake
 final_des = pathDes.getItem(pathDes.count()-1); // get the final face direction

   if(final_des=="1")
  return 1;
   if(final_des=="3")
  return 3; 
  if(final_des=="5")
  return 5;
   if(final_des=="7")
  return 7;  
}

int i,j;
Coordinate startingPoint =graph.node[4][7]; //starting point for maze solving
int countBox=0,countA=0;
QueueList2<Coordinate> path;
QueueList<String> pathDes; //path description
Coordinate target;
Coordinate start = graph.node[dimx-1][dimy-1]; //lower right corner
Coordinate cur,next;
int final_face;

void setup(){
  Serial.begin(9600); //motor declaration is already in movement class
  pinMode(LED_L,OUTPUT);
  pinMode(LED_M,OUTPUT);
  pinMode(LED_R,OUTPUT);
  pinMode(LED_L2,OUTPUT);
  pinMode(LED_R2,OUTPUT);
  pinMode(LED_L3,OUTPUT);
  pinMode(LED_R3,OUTPUT);
  pinMode(LED_FL,OUTPUT);
  pinMode(LED_FR,OUTPUT);
  pinMode(LED_B,OUTPUT);
  pinMode(LED_T,OUTPUT);
  //pinMode(LED_MotorL,OUTPUT);
  //pinMode(LED_MotorR,OUTPUT);
  //node declaration
  Coordinate startNode_D = graph.node[0][0]; // at the start of dryrun we place the bot at node[0][0]
  
  robot.setPos(0,0);  // initial position 0 0-> if the initial has to be 4,7 go to 0,0 first and then start the dryrun
  robot.setface_Dir(2); // face east  
  while(digitalRead(switch_D)==1){ //if the dryrun switch is pressed, do the dryrun
  }
  delay(1000);
  //if it reaches white node, do nothing
  // DRYRUN STARTS HERE
  if(reachBlackNode){
    graph.setBlackNode(0,0);
  }  
  //walk in zigzag manner, for more information, see document.
  for(j=0;j<dimy;j++){
    if(j%2==0){
      for(i=0;i<dimx;i++){//go down  
        robot.setPos(i,j);
        if(i==0){
          Turn_Dryrun(2,4,mediumPower,movement);          
          moveBackUntilReachNode(movement);
          robot.setface_Dir(4); 
          moveAndUpdate(i,j,4,graph,movement);
        }
        else if(i==dimx-1) {//go right
          Turn_Dryrun(4,2,mediumPower,movement);
          moveBackUntilReachNode(movement);
          robot.setface_Dir(2); 
          moveAndUpdate(i,j,2,graph,movement);
        }
        else{
          moveAndUpdate(i,j,4,graph,movement); 
        }
      }
    }
    else {//j is odd
      for(i=dimx-1;i>=0;i--){//go up
        robot.setPos(i,j);
        if(i==dimx-1){
          Turn_Dryrun(2,0,mediumPower,movement);
          moveBackUntilReachNode(movement);
          robot.setface_Dir(0); 
          moveAndUpdate(i,j,0,graph,movement);
        }
        else if(i==0){
          Turn_Dryrun(0,2,mediumPower,movement);
          moveBackUntilReachNode(movement);
          robot.setface_Dir(2);
          if(j!=dimy-1){  
            moveAndUpdate(i,j,2,graph,movement);
          }  
          else{//j==7 last node
            Serial.print("arrive at last node");
          }
        }
        else{ 
          moveAndUpdate(i,j,0,graph,movement);
        }
      }
    }
  }//after the end of this loop, robot will be at node[0][7] facing east face_Dir==3
  // end of dryRun, the bot will be manually replaced at node[4][7]
  while(digitalRead(switch_M)==0){
  }
  delay(1000);
  path = graph.shortPath(startingPoint,start); //from node[4][7] to node[7][7] initially facing south
  pathDes = graph.pathDescription(path,4);  //!!!!!!!!!!!!!!!!!!!! DONT FORGET TO PLACE THE ROBOT SOUTH!!!
 final_face = walk (pathDes,mazemove); // get the final face direction and walk to node[7][7]
 switch(final_face){
    case 0: mazemove.goRight90(); break;  //turn its face to E direction
    case 4: mazemove.goLeft90(); break; 
    } 
    // the bot will move to node[7][7], facing east(3);
    // START MAZE SOLVING
   while(countBox<3){ // while every box is not collected
    robot.setface_Dir(2); // face east
    while(checkBox()==0){
      //do nothing
    }
    if(checkBox()==1){//typeA
      pickBox();
      countBox++;
      if(countA==0){
        target=graph.node[dimx-1][0];
      }
      else{
        target=graph.node[0][0];
      }
      countA++;
    }
    if(checkBox()==2){//typeB
      pickBox();
      countBox++;
      target=node[4][4];
    }
    path = graph.shortPath(start,target);
    pathDes = graph.pathDescription(path,2);
    final_face = walk (pathDes,mazemove); // walk to target and get the final face direction
    //final_face the final face direction
    releaseBox(); //release box when it reaches the target node
    if(countBox==3){
     path = graph.shortPath(target,startingPoint); // if all box is collected, go back to node[4][7] 
    }
    else{
     path = graph.shortPath(target,start); //otherwise, go to node[7][7] to ready for new box
    }
    //กำหนดขากลับ
    pathDes = graph.pathDescription(path,final_face);
    final_face = walk (pathDes,mazemove); // walk to target and get the final face direction
 switch(final_face){
    case 0:  mazemove.goRight90(); break;    //turn to east(3) and ready to receive new box
    case 2:  break;
    case 4:  mazemove.goLeft90(); break;
    } 
  }
}//end of setup
void pickBox(){
}
void releaseBox(){
}
//box counting
void loop(){
 
}

